#include <earth_rover_localization/status.h>

Status::Status(const ros::NodeHandle &nh):
m_nh(nh)
{
    // Load local parameters
    float status_pub_frequency;
    std::string gnss_ref_topic, gnss_att_topic, wheel_odom_topic, imu_topic, status_topic;
    if (!nh.getParam("/status/gnss_reference_topic", gnss_ref_topic))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/gnss_reference_topic");
    }
    if (!nh.getParam("/status/gnss_attitude_topic", gnss_att_topic))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/gnss_attitude_topic");
    }
    if (!nh.getParam("/status/wheel_odometry_topic", wheel_odom_topic))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/wheel_odometry_topic");
    }
    if (!nh.getParam("/status/imu_topic", imu_topic))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/imu_topic");
    }
    if (!nh.getParam("/status/status_topic", status_topic))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/status_topic");
    }
    if (!nh.getParam("/status/status_publisher_frequency", status_pub_frequency))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/status_publisher_frequency");
    }

    // Load global par
    if (!nh.getParam("/status/timeout_gnss_ref", m_timeout_gnss_ref))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/timeout_gnss_ref");
    }
    if (!nh.getParam("/status/timeout_gnss_att", m_timeout_gnss_att))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/timeout_gnss_att");
    }
    if (!nh.getParam("/status/timeout_wheel_odom", m_timeout_wheel_odom))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/timeout_wheel_odom");
    }
    if (!nh.getParam("/status/timeout_imu", m_timeout_imu))
    {
        ROS_ERROR("[LOCALIZATION][STATUS] Missing parameter: /status/timeout_imu");
    }

    // Initialize publishers and subscribers
    m_status_pub = m_nh.advertise<earth_rover_common_messages::LocalizationStatus>(status_topic, 1);

    m_gnss_ref_sub = m_nh.subscribe(gnss_ref_topic, 1, &Status::gnssReferenceCb, this);
    m_gnss_att_sub = m_nh.subscribe(gnss_att_topic, 1, &Status::gnssAttitudeCb, this);
    m_wheel_odom_sub = m_nh.subscribe(wheel_odom_topic, 1, &Status::wheelOdometryCb, this);
    m_imu_sub = m_nh.subscribe(imu_topic, 1, &Status::imuCb, this);

    // Start timer to publish localization status
    m_timer_status_pub = m_nh.createTimer(ros::Duration(1/status_pub_frequency), &Status::statusPublisherCb, this);

    ROS_INFO("[LOCALIZATION][STATUS] Node initialized");
}


void Status::gnssReferenceCb(const piksi_rtk_msgs::ReceiverState_V2_4_1& msg)
{
    // Let watchdog know messages are still arriving
    m_watchdog_time_gnss_ref = ros::Time::now();

    // Start watchdog with specified timeout
    if (!m_gnss_ref_messages_started)
    {
        m_gnss_ref_messages_started = true;
        m_timer_gnss_ref = m_nh.createTimer(ros::Duration(m_timeout_gnss_ref), &Status::watchdogGnssReferenceCb, this);
    }

    // String comparison and string assingment
    if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_INVALID)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_INVALID;
    }
    else if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SPP)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SPP;
    }
    else if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SBAS)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SBAS;
    }
    else if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_DGNSS)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_DGNSS;
    }
    else if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FLOAT_RTK)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FLOAT_RTK;
    }
    else if (msg.fix_mode == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FIXED_RTK)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FIXED_RTK;
    }
    else
    {
        ROS_WARN("[LOCALIZATION][STATUS][GNSS_REFERENCE] Unknown GNSS Reference state");
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_UNKNOWN;
    }
}

void Status::gnssAttitudeCb(const piksi_rtk_msgs::BaselineHeading& msg)
{
    // Let watchdog know messages are still arriving
    m_watchdog_time_gnss_att = ros::Time::now();

    // Start watchdog with specified timeout
    if (!m_gnss_att_messages_started)
    {
        m_gnss_att_messages_started = true;
        m_timer_gnss_att = m_nh.createTimer(ros::Duration(m_timeout_gnss_att), &Status::watchdogGnssAttitudeCb, this);
    }

    // Int comaprison and string assignment
    if (msg.flags == INT_GNSS_ATTITUDE_INVALID)
    {
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_INVALID;
    }
    else if (msg.flags == INT_GNSS_ATTITUDE_RESERVED)
    {
        ROS_WARN("[LOCALIZATION][STATUS][GNSS_ATTITUDE] Reserved GNSS Attitude state");
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_UNKNOWN;
    }
    else if (msg.flags == INT_GNSS_ATTITUDE_DGNSS)
    {
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_DGNSS;
    }
    else if (msg.flags == INT_GNSS_ATTITUDE_FLOAT_RTK)
    {
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_FLOAT_RTK;
    }
    else if (msg.flags == INT_GNSS_ATTITUDE_FIXED_RTK)
    {
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_FIXED_RTK;
    }
    else
    {
        ROS_WARN("[LOCALIZATION][STATUS][GNSS_ATTITUDE] Unknown GNSS Attitude state");
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_UNKNOWN;
    }
}

void Status::wheelOdometryCb(const nav_msgs::Odometry& msg)
{
    // Let watchdag know messages are still arriving
    m_watchdog_time_wheel_odom = ros::Time::now();

    // Start watchdog with specified timeout
    if (!m_wheel_odom_messages_started)
    {
        m_wheel_odom_messages_started = true;
        m_timer_wheel_odom = m_nh.createTimer(ros::Duration(m_timeout_wheel_odom), &Status::watchdogWheelOdometryCb, this);
    }

    m_localization_status.wheel_odometry = earth_rover_common_messages::LocalizationStatus::WHEEL_ODOMETRY_CONNECTED;
}

void Status::imuCb(const sensor_msgs::Imu& msg)
{
    // Let watchdag know messages are still arriving
    m_watchdog_time_imu = ros::Time::now();

    // Start watchdog with specified timeout
    if (!m_imu_messages_started)
    {
        m_imu_messages_started = true;
        m_timer_imu = m_nh.createTimer(ros::Duration(m_timeout_imu), &Status::watchdogImuCb, this);
    }

    m_localization_status.imu = earth_rover_common_messages::LocalizationStatus::IMU_CONNECTED;
}

void Status::statusPublisherCb(const ros::TimerEvent& e)
{
    m_localization_status.header.stamp = ros::Time::now();
    m_status_pub.publish(m_localization_status);
}

void Status::watchdogGnssReferenceCb(const ros::TimerEvent& e)
{
    if ((ros::Time::now() - m_watchdog_time_gnss_ref).toSec() > m_timeout_gnss_ref)
    {
        m_localization_status.gnss_reference = earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_DISCONNECTED;
        ROS_WARN("[LOCALIZATION][STATUS][GNSS_REFERENCE] Hearbeat failed, watchdog triggered after %f s", m_timeout_gnss_ref);
    }
}

void Status::watchdogGnssAttitudeCb(const ros::TimerEvent& e)
{
    if ((ros::Time::now() - m_watchdog_time_gnss_att).toSec() > m_timeout_gnss_att)
    {
        m_localization_status.gnss_attitude = earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_DISCONNECTED;
        ROS_WARN("[LOCALIZATION][STATUS][GNSS_ATTITUDE] Hearbeat failed, watchdog triggered after %f s", m_timeout_gnss_att);
    }
}

void Status::watchdogWheelOdometryCb(const ros::TimerEvent& e)
{
    if ((ros::Time::now() - m_watchdog_time_wheel_odom).toSec() > m_timeout_wheel_odom)
    {
        m_localization_status.wheel_odometry = earth_rover_common_messages::LocalizationStatus::WHEEL_ODOMETRY_DISCONNECTED;
        ROS_WARN("[LOCALIZATION][STATUS][WHEEL_ODOMETRY] Hearbeat failed, watchdog triggered after %f s", m_timeout_wheel_odom);
    }
}

void Status::watchdogImuCb(const ros::TimerEvent& e)
{
    if ((ros::Time::now() - m_watchdog_time_imu).toSec() > m_timeout_imu)
    {
        m_localization_status.imu = earth_rover_common_messages::LocalizationStatus::IMU_DISCONNECTED;
        ROS_WARN("[LOCALIZATION][STATUS][IMU] Hearbeat failed, watchdog triggered after %f s", m_timeout_imu);
    }
}
